#include "HT03_driver.h"
#include <stdlib.h>
#include "BSP.h"

#define LIHT_MIN_MAX(x, min, max) (x) = (((x) <= (min)) ? (min) : (((x) >= (max)) ? (max) : (x)))

/* HT电机模式代码 */
enum HT_Commands
{
    CMD_ENTER_MOTOR_MODE = 0xFC,
    CMD_EXIT_MOTOR_MODE = 0xFD,
    CMD_SET_MOTOR_ZEROPOSITION = 0xFE
};

/**
 * 映射设置的float参数到HT电机适配参数
 * 16 bit position command
 * 12 bit velocity command
 * 12 bit Kp
 * 12 bit Kd
 * 12 bit Feed-Forward Current 前馈电流
 */
static inline uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
{
    float span = x_max - x_min;
    float offset = x_min;

    return (uint16_t)((x - offset) * ((float)((1 << bits) - 1)) / span);
}

/**
 * CAN接收数据映射到float类型
 */
static inline float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
    float span = x_max - x_min;
    float offset = x_min;
    return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
}

// 设置电机参数
static void HT_SendParameter(uint8_t ID, float angle, float speed, float Kp, float Kd, float torque)
{
    uint16_t p, v, kp, kd, t;
    uint8_t buf[8];

    /* 限制输入的参数在定义的范围内 */
    LIHT_MIN_MAX(angle, P_MIN, P_MAX);
    LIHT_MIN_MAX(speed, V_MIN, V_MAX);
    LIHT_MIN_MAX(Kp, KP_MIN, KP_MAX);
    LIHT_MIN_MAX(Kd, KD_MIN, KD_MAX);
    LIHT_MIN_MAX(torque, T_MIN, T_MAX);

    /* 根据协议，对float参数进行转换 */
    p = float_to_uint(angle, P_MIN, P_MAX, 16);
    v = float_to_uint(speed, V_MIN, V_MAX, 12);
    kp = float_to_uint(Kp, KP_MIN, KP_MAX, 12);
    kd = float_to_uint(Kd, KD_MIN, KD_MAX, 12);
    t = float_to_uint(torque, T_MIN, T_MAX, 12);

    /* 根据传输协议，把数据转换为CAN命令数据字段 */
    buf[0] = p >> 8;
    buf[1] = p & 0xFF;
    buf[2] = v >> 4;
    buf[3] = ((v & 0xF) << 4) | (kp >> 8);
    buf[4] = kp & 0xFF;
    buf[5] = kd >> 4;
    buf[6] = ((kd & 0xF) << 4) | (t >> 8);
    buf[7] = t & 0xff;

    CAN_Transmit(&HT_CAN, ID, buf);
}

static HT_measure_t **MotorsArray; // 电机指针数组
static uint8_t MotorsCount;        // 电机数量

// CAN中断回调函数
void HT_CAN_Callback(CAN_RxHeaderTypeDef *pHeader, uint8_t *pBuf)
{
    if (pHeader->StdId == 0x00)
    {
        for (uint8_t i = 0; i < MotorsCount; i++)
        {
            if (MotorsArray[i]->ID == pBuf[0])
            {
                // 接收数据转换
                MotorsArray[i]->Position = uint_to_float((uint16_t)(pBuf[1] << 8 | pBuf[2]), P_MIN, P_MAX, 16);
                MotorsArray[i]->Velocity = uint_to_float((uint16_t)(pBuf[3] << 4 | pBuf[4] >> 4), V_MIN, V_MAX, 12);
                MotorsArray[i]->Torque = uint_to_float((uint16_t)((pBuf[4] & 0x0F) << 8 | pBuf[5]), T_MIN, T_MAX, 12);
            }
        }
    }
}

static uint8_t cmd[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00};

void HT_Init(HT_measure_t *Motor, uint8_t ID, float Kp, float Kd)
{
    // 进入电机模式
    cmd[7] = CMD_ENTER_MOTOR_MODE;
    CAN_Transmit(&HT_CAN, ID, cmd);
    HAL_Delay(1);

    // 设置零参数
    HT_SendParameter(ID, 0, 0, 0, 0, 0);
    HAL_Delay(1);

    // 设置电机零位置
    cmd[7] = CMD_SET_MOTOR_ZEROPOSITION;
    CAN_Transmit(&HT_CAN, ID, cmd);
    HAL_Delay(1);

    MotorsCount++; // 电机数+1
    MotorsArray = (HT_measure_t **)realloc(MotorsArray, MotorsCount * sizeof(HT_measure_t *));
    MotorsArray[MotorsCount - 1] = Motor;
    Motor->ID = ID;
    Motor->Kp = Kp;
    Motor->Kd = Kd;
}

void HT_SetZeroPosition(HT_measure_t *Motor)
{
    cmd[7] = CMD_SET_MOTOR_ZEROPOSITION;
    CAN_Transmit(&HT_CAN, Motor->ID, cmd);
}

inline void HT_SetSpeed(HT_measure_t *Motor, float speed)
{
    HT_SendParameter(Motor->ID, 0, speed, 0, Motor->Kd, 0);
}

inline void HT_SetAngle(HT_measure_t *Motor, float angle)
{
    HT_SendParameter(Motor->ID, angle, 0, Motor->Kp, Motor->Kd, 0);
}
